import numpy as np

# 工具函数
def euc_dist(p1, p2):
    return np.sqrt(np.sum((np.array(p1) - np.array(p2)) ** 2))

def comp_dist_mtx(locs):
    n = locs.shape[0]
    mtx = np.zeros((n, n))
    for i in range(n):
        for j in range(n):
            if i != j:
                mtx[i, j] = euc_dist(locs[i], locs[j])
    return mtx

def calc_path_dist(mtx, path):
    dist = 0
    n = len(path)
    for i in range(n):
        dist += mtx[path[i], path[(i + 1) % n]]
    return float(dist)

# 算法实现
def nearest_neighbor(mtx, start=0):
    n = mtx.shape[0]
    visited = [False] * n
    path = [start]
    visited[start] = True
    curr = start
    for _ in range(n - 1):
        nearest = None
        min_dist = float('inf')
        for city in range(n):
            if not visited[city] and mtx[curr, city] < min_dist:
                nearest = city
                min_dist = mtx[curr, city]
        if nearest is not None:
            path.append(nearest)
            visited[nearest] = True
            curr = nearest
    # 如果还有未访问的城市，则直接添加到路径末尾（这种情况理论上不应该发生）
    for city in range(n):
        if not visited[city]:
            path.append(city)
    return path

def two_point_greedy(mtx, start=0):
    n = mtx.shape[0]
    visited = [False] * n
    path = [start]
    visited[start] = True
    for _ in range(n - 1):
        min_dist = float('inf')
        next_city = None
        for i in range(n):
            if not visited[i]:
                for j in range(n):
                    if i != j and not visited[j]:
                        dist = mtx[i, j]
                        if dist < min_dist:
                            min_dist = dist
                            next_city = i
        if next_city is not None:
            path.append(next_city)
            visited[next_city] = True
        else:
            # 如果找不到合适的下一个城市，添加剩余未访问的城市
            for city in range(n):
                if not visited[city]:
                    path.append(city)
                    visited[city] = True
    return path

def three_point_greedy(mtx, start=0):
    n = mtx.shape[0]
    visited = [False] * n
    path = [start]
    visited[start] = True
    while len(path) < n:
        remaining_cities = [i for i in range(n) if not visited[i]]
        if len(remaining_cities) <= 2:
            # 如果剩余城市不足两个，直接添加它们到路径中
            path.extend(remaining_cities)
            break
        min_dist = float('inf')
        next_city = None
        for i in remaining_cities:
            for j in remaining_cities:
                if i != j:
                    for k in remaining_cities:
                        if i != k and j != k:
                            dist = mtx[path[-1], i] + mtx[i, j]
                            if dist < min_dist:
                                min_dist = dist
                                next_city = i
        if next_city is not None:
            path.append(next_city)
            visited[next_city] = True
        else:
            # 如果找不到合适的下一个城市，添加剩余未访问的城市
            for city in remaining_cities:
                path.append(city)
                visited[city] = True
    return path

def opt_path(mtx, path):
    n = len(path)
    improved = True
    while improved:
        improved = False
        for i in range(n):
            for j in range(i + 1, n):
                if (mtx[path[i], path[(i + 1) % n]] + mtx[path[j], path[(j + 1) % n]] >
                        mtx[path[i], path[j]] + mtx[path[(i + 1) % n], path[(j + 1) % n]]):
                    path = path[:i + 1] + path[j:i:-1] + path[j + 1:]
                    improved = True
    return path

# 主程序
if __name__ == "__main__":
    # 设置随机种子以确保结果可重复
    np.random.seed(42)

    # 控制位置矩阵的规模
    num_cities = 40

    # 随机生成位置矩阵
    locs = np.random.rand(num_cities, 2) * 100  # 生成0到100之间的随机坐标
    print(locs)

    mtx = comp_dist_mtx(locs)

    # 1. 以固定起点的三种算法的结果
    fixed_start = 0
    nn_path = nearest_neighbor(mtx, start=fixed_start)
    tp_greedy_path = two_point_greedy(mtx, start=fixed_start)
    three_p_greedy_path = three_point_greedy(mtx, start=fixed_start)

    print("固定起点结果:")
    print("最近邻路径:")
    print(nn_path)
    print(f"数量:{len(nn_path)},总长度:", calc_path_dist(mtx, nn_path))
    print("两点贪心路径:")
    print(tp_greedy_path)
    print(f"数量:{len(tp_greedy_path)},总长度:", calc_path_dist(mtx, tp_greedy_path))
    print("三点贪心路径:")
    print(three_p_greedy_path)
    print(f"数量:{len(three_p_greedy_path)},总长度:", calc_path_dist(mtx, three_p_greedy_path))
    print()

    # 2. 以固定起点的三种算法的结果，并优化路径的结果
    opt_nn_path = opt_path(mtx, nn_path)
    opt_tp_greedy_path = opt_path(mtx, tp_greedy_path)
    opt_three_p_greedy_path = opt_path(mtx, three_p_greedy_path)

    print("固定起点优化后结果:")
    print("优化后的最近邻路径:")
    print(opt_nn_path)
    print(f"数量:{len(opt_nn_path)},总长度:", calc_path_dist(mtx, opt_nn_path))
    print("优化后的两点贪心路径:")
    print(opt_tp_greedy_path)
    print(f"数量:{len(opt_tp_greedy_path)},总长度:", calc_path_dist(mtx, opt_tp_greedy_path))
    print("优化后的三点贪心路径:")
    print(opt_three_p_greedy_path)
    print(f"数量:{len(opt_three_p_greedy_path)},总长度:", calc_path_dist(mtx, opt_three_p_greedy_path))
    print()

    # 3. 考虑全部起点的三种算法的结果，并找出最优路径
    min_length_nn = float('inf')
    best_nn_path = None
    min_length_tp = float('inf')
    best_tp_path = None
    min_length_three_p = float('inf')
    best_three_p_path = None

    for start in range(locs.shape[0]):
        nn_path = nearest_neighbor(mtx, start=start)
        nn_length = calc_path_dist(mtx, nn_path)
        if nn_length < min_length_nn:
            min_length_nn = nn_length
            best_nn_path = nn_path

        tp_greedy_path = two_point_greedy(mtx, start=start)
        tp_length = calc_path_dist(mtx, tp_greedy_path)
        if tp_length < min_length_tp:
            min_length_tp = tp_length
            best_tp_path = tp_greedy_path

        three_p_greedy_path = three_point_greedy(mtx, start=start)
        three_p_length = calc_path_dist(mtx, three_p_greedy_path)
        if three_p_length < min_length_three_p:
            min_length_three_p = three_p_length
            best_three_p_path = three_p_greedy_path

    print("考虑全部起点的最优结果:")
    print("最优最近邻路径:")
    print(best_nn_path)
    print(f"数量:{len(best_nn_path)},总长度:", min_length_nn)
    print("最优两点贪心路径:")
    print(best_tp_path)
    print(f"数量:{len(best_tp_path)},总长度:", min_length_tp)
    print("最优三点贪心路径:")
    print(best_three_p_path)
    print(f"数量:{len(best_three_p_path)},总长度:", min_length_three_p)
    print()


    # 4. 考虑全部起点的三种算法的结果，并优化路径后找出最优路径
    opt_min_length_nn = float('inf')
    opt_best_nn_path = None
    opt_min_length_tp = float('inf')
    opt_best_tp_path = None
    opt_min_length_three_p = float('inf')
    opt_best_three_p_path = None

    for start in range(locs.shape[0]):
        nn_path = nearest_neighbor(mtx, start=start)
        opt_nn_path = opt_path(mtx, nn_path)
        opt_nn_length = calc_path_dist(mtx, opt_nn_path)
        if opt_nn_length < opt_min_length_nn:
            opt_min_length_nn = opt_nn_length
            opt_best_nn_path = opt_nn_path

        tp_greedy_path = two_point_greedy(mtx, start=start)
        opt_tp_path = opt_path(mtx, tp_greedy_path)
        opt_tp_length = calc_path_dist(mtx, opt_tp_path)
        if opt_tp_length < opt_min_length_tp:
            opt_min_length_tp = opt_tp_length
            opt_best_tp_path = opt_tp_path

        three_p_greedy_path = three_point_greedy(mtx, start=start)
        opt_three_p_path = opt_path(mtx, three_p_greedy_path)
        opt_three_p_length = calc_path_dist(mtx, opt_three_p_path)
        if opt_three_p_length < opt_min_length_three_p:
            opt_min_length_three_p = opt_three_p_length
            opt_best_three_p_path = opt_three_p_path

    print("考虑全部起点优化后的最优结果:")
    print("优化后的最优最近邻路径:")
    print(opt_best_nn_path)
    print(f"数量:{len(opt_best_nn_path)},总长度:", opt_min_length_nn)
    print("优化后的最优两点贪心路径:")
    print(opt_best_tp_path)
    print(f"数量:{len(opt_best_tp_path)},总长度:", opt_min_length_tp)
    print("优化后的最优三点贪心路径:")
    print(opt_best_three_p_path)
    print(f"数量:{len(opt_best_three_p_path)},总长度:", opt_min_length_three_p)
